﻿#include "transform.h"
#include "math/linear_math.h"

#define SIGN(x) ((x) > 0 ? 1 : -1)

// 输出函数
template <int M, int N, typename T> void print_mat(const CAGD::TMatMxN<M, N, T> &mat)
{
    for (int i = 0; i < M; i++)
    {
        for (int j = 0; j < N; j++)
            std::cout << mat(i, j) << " ";
        std::cout << std::endl;
    }
}

// 转换函数
template <int M, int N, typename T> CAGD::Matrix convert(const CAGD::TMatMxN<M, N, T> &mat)
{
    CAGD::Matrix m(M, N);
    for (int i = 0; i < M; i++)
        for (int j = 0; j < N; j++)
            m(i, j) = mat(i, j);
    return m;
}

template <int M, int N, typename T> CAGD::TMatMxN<M, N, T> convert(const CAGD::Matrix &mat)
{
    CAGD::TMatMxN<M, N, T> m;
    for (int i = 0; i < M; i++)
        for (int j = 0; j < N; j++)
            m(i, j) = mat(i, j);
    return m;
}

CAGD::BMat4x4 CAGD::Transform::Inverted(const BMat4x4 &mat)
{
    auto m = convert(mat);
    auto im = m.Invert();
    return convert<4, 4, float>(im);
}

CAGD::BMat4x4 CAGD::Transform::Transposed(const BMat4x4 &mat)
{
    BMat4x4 m;
    for (int i = 0; i < 4; i++)
        for (int j = 0; j < 4; j++)
            m(i, j) = mat(j, i);
    return m;
}

CAGD::BMat4x4 CAGD::Transform::Translate(BVecf d)
{
    BMat4x4 A;

    A(0, 3) = d.X();
    A(1, 3) = d.Y();
    A(2, 3) = d.Z();

    return A;
}

CAGD::BMat4x4 CAGD::Transform::Scale(BPntf pos, BVecf scale)
{
    // 移动到指定位置，然后缩放，再移动回去
    BMat4x4 T1 = Translate(-pos);

    BMat4x4 S;
    S(0, 0) = scale.X();
    S(1, 1) = scale.Y();
    S(2, 2) = scale.Z();

    BMat4x4 T2 = Translate(pos);

    return T2 * S * T1;
}

CAGD::BPntf CAGD::Transform::Perform(const BMat4x4 &trsf, BPntf pos)
{
    BPnt4df v = {pos.X(), pos.Y(), pos.Z(), 1};
    auto r = trsf * v;
    r /= r.W();
    return {r.X(), r.Y(), r.Z()};
}

CAGD::BVecf CAGD::Transform::Perform(const BMat4x4 &trsf, BVecf vec)
{
    BPnt4df v0 = {vec.X(), vec.Y(), vec.Z(), 1};
    BPnt4df v1 = {0, 0, 0, 1};
    auto r0 = trsf * v0;
    auto r1 = trsf * v1;
    r0 /= r0.W();
    r1 /= r1.W();
    auto r = r0 - r1;
    return {r.X(), r.Y(), r.Z()};
}

CAGD::BMat4x4 CAGD::Transform::Perspective(float fovy, float aspect, float zNear, float zFar)
{
    BMat4x4 A;

    // 将平截头体映射为长方体
    A(0, 0) = zNear;
    A(1, 1) = zNear;
    A(2, 2) = zNear + zFar;
    A(2, 3) = zNear * zFar;
    A(3, 2) = -1;
    A(3, 3) = 0;

    // 做正交投影
    float bottom = -zNear * tan(fovy / 360.0 * 3.1415926535);
    float top = -bottom;
    float left = bottom * aspect;
    float right = -left;

    BMat4x4 O = Ortho(left, right, bottom, top, zNear, zFar);
    return O * A;
}

CAGD::BMat4x4 CAGD::Transform::Ortho(float left, float right, float bottom, float top, float zNear, float zFar)
{
    BMat4x4 A;

    A(0, 0) = 2 / (right - left);
    A(1, 1) = 2 / (top - bottom);
    A(2, 2) = -2 / (zFar - zNear);

    A(0, 3) = -(left + right) / (right - left);
    A(1, 3) = -(top + bottom) / (top - bottom);
    A(2, 3) = (zFar + zNear) / -(zFar - zNear);

    return A;
}

CAGD::BMat4x4 CAGD::Transform::LookAt(BPntf pos, BPntf origin, BVecf up)
{
    // 平移矩阵
    BMat4x4 T;
    T(0, 3) = -pos.X();
    T(1, 3) = -pos.Y();
    T(2, 3) = -pos.Z();

    // 计算相机坐标系
    BVecf g = (origin - pos).Normalized();
    BVecf x = (g % up.Normalized()).Normalized();
    BVecf t = x % g;

    // 旋转矩阵
    BMat4x4 R;
    R(0, 0) = x.X();
    R(0, 1) = x.Y();
    R(0, 2) = x.Z();

    R(1, 0) = t.X();
    R(1, 1) = t.Y();
    R(1, 2) = t.Z();

    R(2, 0) = -g.X();
    R(2, 1) = -g.Y();
    R(2, 2) = -g.Z();

    return R * T;
}